package net.neocat.tcx.file;

import static java.lang.Math.*;

public class GeoPosition {
    private final double x;
    private final double y;
    private final double z;

    public GeoPosition(double longitude, double latitude, double elevationMeters) {
        this.x = longitude;
        this.y = latitude;
        this.z = elevationMeters;
    }

    public double getElevationMeters() {
        return z;
    }

    public double getLongitude() {
        return x;
    }

    public double getLatitude() {
        return y;
    }

    public double getX() {
        return x;
    }

    public double getY() {
        return y;
    }

    public double getZ() {
        return z;
    }

    @Override
    public boolean equals(Object o) {
        if (this == o) return true;
        if (o == null || getClass() != o.getClass()) return false;

        GeoPosition position = (GeoPosition) o;

        if (Double.compare(position.y, y) != 0) return false;
        if (Double.compare(position.x, x) != 0) return false;

        return true;
    }

    @Override
    public int hashCode() {
        int result;
        long temp;
        temp = Double.doubleToLongBits(x);
        result = (int) (temp ^ (temp >>> 32));
        temp = Double.doubleToLongBits(y);
        result = 31 * result + (int) (temp ^ (temp >>> 32));
        return result;
    }

    @Override
    public String toString() {
        final StringBuilder sb = new StringBuilder("GeoPosition{");
        sb.append("x=").append(x);
        sb.append(", y=").append(y);
        sb.append(", z=").append(z);
        sb.append('}');
        return sb.toString();
    }

    /**
     * Finds the squared distance between two Tuples. This is useful when comparing
     * distances because it avoids a square root.
     *
     * @param rhs
     * @return squared distance
     */
    public double distanceSquared2D(GeoPosition rhs) {
        return (x - rhs.x) * (x - rhs.x) + (y - rhs.y) * (y - rhs.y);
    }

    /**
     * Finds the squared distance between two Tuples. This is useful when comparing
     * distances because it avoids a square root.
     *
     * @param rhs
     * @return squared distance
     */
    public double distance2D(GeoPosition rhs) {
        return sqrt(distanceSquared2D(rhs));
    }

    public double length2D() {
        return (float) sqrt(x * x + y * y);
    }

    /**
     * Subtract two tuples and return the value in rhs new Tuple
     *
     * @param rhs
     */
    public GeoPosition minus(GeoPosition rhs) {
        return new GeoPosition(x - rhs.x, y - rhs.y, z - rhs.z);
    }

    /**
     * Add two tuples and return the value in a new Tuple
     *
     */
    public GeoPosition plus(double dx, double dy, double dz) {
        return new GeoPosition(x + dx, y + dy, z + dz);
    }

    /**
     * Add two tuples and return the value in a new Tuple
     *
     * @param rhs
     */
    public GeoPosition plus(GeoPosition rhs) {
        return new GeoPosition(x + rhs.x, y + rhs.y, z + rhs.z);
    }

    /**
     * In place divide
     *
     * @param a divisor
     */
    public GeoPosition divide(double a) {
        return new GeoPosition(x / a, y / a, z / a);
    }

    /**
     * Returns the dot product of two Tuples.  Generally, this can b interpreted as the
     * angle between them.
     *
     * @param rhs compare to this Tuple
     * @return the dot product
     */
    public double dot(GeoPosition rhs) {
        return (x * rhs.x) + (y * rhs.y);
    }

    public GeoPosition times(double rhs) {
        return new GeoPosition(x * rhs, y * rhs, z * rhs);
    }

    private static final double EARTH_RADIUS_METERS = 6371009;

    public double greatCircleDistanceMeters2D(GeoPosition rhs) {
        final double lon1 = this.getLongitude();
        final double lat1 = this.getLatitude();
        final double lon2 = rhs.getLongitude();
        final double lat2 = rhs.getLatitude();

        double latDistance = toRadians(lat2 - lat1);
        double lonDistance = toRadians(lon2 - lon1);
        double a = sin(latDistance / 2) * sin(latDistance / 2)
                + cos(toRadians(lat1)) * cos(toRadians(lat2)) * sin(lonDistance / 2) * sin(lonDistance / 2);
        double c = 2 * atan2(sqrt(a), sqrt(1 - a));
        return EARTH_RADIUS_METERS * c;
    }
}
